#ifndef     __PRIORITY_QUEUE_H
#define     __PRIORITY_QUEUE_H

#include "./event_flags.h"
#include <utility>

namespace rtos {

template <typename T, uint32_t CAPACITY>
class PriorityQueue : private utils::NoCopyable {

public:
    static constexpr uint32_t FLAG_LOCK = 0x01;
    static constexpr uint32_t FLAG_COND = 0x02;

    struct Item {
        uint32_t tick;
        uint32_t delay;
        T value;
    };

    void init() {
        m_flags.init();
        unlock();
    }

    bool post(uint32_t delay, T&& value) {
        lock();
        if (m_size >= CAPACITY) {
            unlock();
            return false;
        }
        m_data[m_size].tick = osKernelGetTickCount();
        m_data[m_size].delay = delay;
        m_data[m_size].value = std::move(value);
        m_size += 1;
        notify();
        unlock();
        return true;
    }

    T poll() {
        lock(); 
        for (;;) {

            uint32_t current = osKernelGetTickCount();
            uint32_t min_delay = osWaitForever;
            
            for (uint32_t i = 0; i < m_size; i ++) {
                uint32_t elapsed = current - m_data[i].tick;
                if (elapsed >= m_data[i].delay) {
                    T value = remove(i);
                    unlock();
                    return value;
                }
                uint32_t n = m_data[i].delay - elapsed;
                if (n < min_delay) {
                    min_delay = n;
                }
            }

            wait(min_delay);
        }
    }

private:
    T remove(uint32_t index) {
        T value = std::move(m_data[index]).value;
        if (m_size > 1) {
            m_data[index] = std::move(m_data[m_size - 1]);
        }
        m_size -= 1;
        return value;
    }

    inline void lock() {
        m_flags.wait_any_flags(FLAG_LOCK);
    }

    inline void unlock() {
        m_flags.set_flags(FLAG_LOCK);
    }

    inline void wait(uint32_t timeout = osWaitForever) {
        unlock();
        m_flags.wait_any_flags(FLAG_COND, timeout);
        lock();
    }

    inline void notify() {
        m_flags.set_flags(FLAG_COND);
    }

    EventFlags m_flags;
    Item m_data[CAPACITY];
    uint32_t m_size = 0;
};

}


#endif
